#! /usr/bin/env python

# Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland
# You can contact the author at <slynen at ethz dot ch>
# Copyright (C) 2011-2012 Stephan Weiss, ASL, ETH Zurich, Switzerland
# You can contact the author at <stephan dot weiss at ieee dot org>
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

PACKAGE='msf_core'
import roslib; roslib.load_manifest(PACKAGE)

#from driver_base.msg import SensorLevels
from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# @todo Think about levels. Setting most of these to STOP to guarantee atomicity.
# @todo Double check these ranges, defaults


# The IMU measurement model used in msf contains two types of sensor errors, a high frequency additive white noise and 
# a slower varying sensor bias.  
# See the following link for more information 
# https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics
# 
# The white noise is characterized with the continuous time noise spectral density. 
# The noise spectral density is sometime also referred to as noise density.
# The units of the noise spectral density are:
#  acc:  [m/s^2/sqrt(Hz)]
#  gyro: [rad/s/sqrt(Hz)]
# The noise spectral density can be found in the datasheet of the IMU.
# 
# The variation of the bias is characterized as a random walk. 
# Again see https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics for more information
# The units of the random walk are:
#  acc:  [m/s^3/sqrt(Hz)]
#  gyro: [rad/s^2/sqrt(Hz)]

#       Name                 Type      Reconfiguration level                Description   Default   Min   Max
gen.add("core_fixed_bias",        bool_t,   0,                           "fix biases",                    False)
gen.add("core_noise_acc",         double_t, 0,                           "accelerations noise spectral density (nsd) [m/s^2/sqrt(Hz)]",       0.002,   	1.0e-4, 	0.5)
gen.add("core_noise_accbias",     double_t, 0,                           "acceleration biases random walk [m/s^3/sqrt(Hz)]", 5e-8, 	1.0e-8, 	0.1)
gen.add("core_noise_gyr",         double_t, 0,                           "gyros noise spectral density (nsd) [rad/s/sqrt(Hz)]",                 0.0004,  	1.0e-4,   	0.5)
gen.add("core_noise_gyrbias",     double_t, 0,                           "gyro biases random walk [rad/s^2/sqrt(Hz)]",                 3e-6, 	1.0e-7, 	0.1)

exit(gen.generate(PACKAGE, "Config", "MSF_Core"))
